#ifndef HEBUILDER_H
#define HEBUILDER_H

#include <vector>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/Polyhedron_incremental_builder_3.h>
#include "geometricprimitives.h"

using namespace std;

// A new vertex type with a plugged normal.
template <class Refs , class P>
struct My_vertex : public CGAL::HalfedgeDS_vertex_base<Refs, CGAL::Tag_true, P> {
    Vector normal;
    //Constructors only call the parents'.
    My_vertex(): CGAL::HalfedgeDS_vertex_base<Refs, CGAL::Tag_true, P>(){}
    My_vertex(const Point& p): CGAL::HalfedgeDS_vertex_base<Refs, CGAL::Tag_true, P>(p){}
};

// A new face type with a plugged area value.
template <class Refs , class Traits>
struct My_face : public CGAL::HalfedgeDS_face_base<Refs , CGAL::Tag_true , Plane> {
    double dArea;
    //Constructors only call the parents'.
    My_face(): CGAL::HalfedgeDS_face_base<Refs, CGAL::Tag_true, Plane>(){}
    My_face(const Plane& plane): CGAL::HalfedgeDS_face_base<Refs, CGAL::Tag_true, Plane>(plane){}
};


//A new Polyhedron_items_3 type with the new vertex definition plugged.
struct My_items : public CGAL::Polyhedron_items_3 {
    template <class Refs, class Traits>
    struct Vertex_wrapper {
        typedef typename Traits::Point_3 Point;
        typedef My_vertex<Refs , Point> Vertex;
    };

    template <class Refs, class Traits>
    struct Face_wrapper {
        typedef typename Traits::Plane_3 Plane;
        typedef My_face<Refs , Plane> Face;
    };
};

typedef CGAL::Simple_cartesian<double>          K;
typedef CGAL::Vector_3<K>                       Vector;
typedef CGAL::Triangle_3<K>                     Triangle;
typedef CGAL::Polyhedron_3<K , My_items>        Poly;
typedef Poly::HalfedgeDS                        HalfedgeDS;
typedef Poly::Halfedge_handle                   Halfedge_handle;
typedef Poly::Facet_handle                      Facet_handle;
typedef Poly::Vertex_iterator                   Vertex_iterator;
typedef Poly::Facet_iterator                    Facet_iterator;
typedef Poly::Halfedge_around_vertex_circulator Halfedge_vertex_circulator;

//Functor that tells how to compute the plane equations of each facet.
struct Plane_equation {
    template <class Facet>
    typename Facet::Plane_3 operator()( Facet& f) {
        typename Facet::Halfedge_handle h = f.halfedge();
        typedef typename Facet::Plane_3  Plane;
        return Plane( h->vertex()->point(),
                      h->next()->vertex()->point(),
                      h->next()->next()->vertex()->point());
    }
};

// A modifier creating the halfedge with the incremental builder.
template <class HDS>
class HeBuilder : public CGAL::Modifier_base<HDS> {
public:
    long m_numTriangles;
    vector<TRIANGLE> *m_vTriangles;

    HeBuilder(vector<TRIANGLE> *vTriangles , long numTriangles){
        m_numTriangles = numTriangles;
        m_vTriangles = vTriangles;
    }
    void operator()( HDS& hds){
        //Constructs a halfedge from a polygon soup of triangles. Uses one aux map which associate vertices coordinates with
        //vertices indices.
        stringstream keyV;
        keyV.precision(10);

        typedef map<string,int> MapVertices;
        MapVertices mapVertices;
        MapVertices::iterator vIt;
        int aiTriangleVIndices[3];

        CGAL::Polyhedron_incremental_builder_3<HDS> B( hds, true);
        B.begin_surface( m_numTriangles, m_numTriangles);
        typedef typename HDS::Vertex   Vertex;
        typedef typename Vertex::Point Point;

        int iNumVertex = 0;

        K::Triangle_3 testTriangle;
        K::Point_3 testTriangleVs[3];

        for(vector<TRIANGLE>::iterator it = m_vTriangles->begin() ; it != m_vTriangles->end() ; it++){
            for(int i = 0 ; i < 3 ; i++){
                testTriangleVs[i] = Point( it->p[i].x , it->p[i].y , it->p[i].z); //triangle test
                keyV.str(string());

                keyV << fixed << "(" << it->p[i].x << "," << it->p[i].y << "," << it->p[i].z << ")";
                vIt = mapVertices.find(keyV.str());
                if(vIt != mapVertices.end()){
                    //Vertex already added. Getting the index.
                    aiTriangleVIndices[i] = vIt->second;
                }
                else{
                    //Add vertex and get the new index.
                    B.add_vertex(Point( it->p[i].x , it->p[i].y , it->p[i].z));
                    mapVertices[keyV.str()] = iNumVertex;
                    aiTriangleVIndices[i] = iNumVertex++;
                }
            };

            //triangle test
            testTriangle = K::Triangle_3(testTriangleVs[0] , testTriangleVs[1] , testTriangleVs[2]);
            if(testTriangle.is_degenerate()){
                //cout << "degenerated triangle..." << endl;
            }
            else{
                //Adding facet
                B.begin_facet();
                //cout << "creating facet (" << aiTriangleVIndices[0] << "," << aiTriangleVIndices[1] << "," << aiTriangleVIndices[2] << ")" << endl;
                B.add_vertex_to_facet(aiTriangleVIndices[0]);
                B.add_vertex_to_facet(aiTriangleVIndices[1]);
                B.add_vertex_to_facet(aiTriangleVIndices[2]);
                B.end_facet();
            }
        }
        B.end_surface();
    }
};
#endif // HEBUILDER_H
